#ifndef __BALANCE_CONTROL_H__
#define __BALANCE_CONTROL_H__

#define INTERRUPT0_BALANCE_CONTROL_TIME 10
#define BALANCE_CONTROL_P               -800
#define BALANCE_CONTROL_I               0
#define BALANCE_CONTROL_D               -4800
#define BALANCE_CONTROL_INTEGRAL_LIMIT  0
#define BALANCE_CONTROL_MAX_OUTPUT      5000
#define BALANCE_CONTROL_TARGET_ANGLE    43   //****************************************  need to be measured
#define BALANCE_CONTROL_K               1    //complementary filtering factor

typedef struct _BALANCE_CONTROL_t
{
    double Acc_Z,Acc_X,Gyro_Y;
    double Gyro_Y_zero;
    float balance_control_k;
    float balance_control_current_angle;
    float balance_control_target_angle;
    PID_t balance_control_pid;
    float balance_control_output;

    void (*f_init_balance_control)(struct _BALANCE_CONTROL_t *balance_control);
    void (*f_update_attitude)(struct _BALANCE_CONTROL_t *balance_control);
    void (*f_ZeroDrift)(struct _BALANCE_CONTROL_t *balance_control);

}BALANCE_CONTROL_t;

void BALANCE_CONTROL_StructInit(BALANCE_CONTROL_t *balance_control);
int16_t fast_atan2(float real, float imag);
#endif 
